/**
 * @file graph.cpp
 * @author 范文宇 (2643660853@qq.com)
 * @brief 
 * @version 1.0
 * @date 2022-10-21
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include "graph.h"
using namespace std;
using namespace cv;

Graph::Graph(int point_width, int point_height,
                 cv::Point2i startingPoint, cv::Point2i finishingPoint)
{
    this->__point_width = point_width;                
    this->__point_height = point_height;              
    this->__startingPoint = startingPoint;            
    this->__finishingPoint = finishingPoint;          
    __vertexNum = __point_width * __point_height;     
    __vertex.reserve(__vertexNum);                    
    __obstacle.reserve(__vertexNum);                  
    __edge = Mat(__vertexNum, __vertexNum, CV_16FC1); 
  
    for (int i = 0; i < __vertexNum; i++)
    {
        __vertex.push_back(i);
    }
    
    initEdge();
}
void Graph::initEdge()
{
    for (int i = 0; i < __vertexNum; i++)
    {
        for (int j = 0; j < __vertexNum; j++)
        {
            
            if (i == j)
            {
                __edge.at<double_t>(i, j) = 0;
            }
            
            else if ((abs(i - j) == 1 || abs(i - j) == __point_width))
            {
                __edge.at<double_t>(i, j) = 1;
                __edge.at<double_t>(j, i) = 1;
            }
            else if (abs(i - j) == __point_width + 1 || abs(i - j) == __point_width - 1)
            {
                __edge.at<double_t>(i, j) = 1.41421;
                __edge.at<double_t>(j, i) = 1.41421;
            }
            else
            {
                __edge.at<double_t>(i, j) = 1e7;
                __edge.at<double_t>(j, i) = 1e7;
            }
        }
        if (i % __point_width == 0)
        {
            if (i - __point_width - 1 > -1)
            {
                __edge.at<double_t>(i, i - __point_width - 1) = 1e7;
                __edge.at<double_t>(i - __point_width - 1, i) = 1e7;
            }
         
            if (i - 1 > -1)
            {
                __edge.at<double_t>(i, i - 1) = 1e7;
                __edge.at<double_t>(i - 1, i) = 1e7;
            }
            
            if (i + __point_width - 1 < __point_height * __point_width)
            {
                __edge.at<double_t>(i, i + __point_width - 1) = 1e7;
                __edge.at<double_t>(i + __point_width - 1, i) = 1e7;
            }
        }
    
        if (i % __point_width == __point_width - 1)
        {
            
            if (i - __point_width + 1 > -1)
            {
                __edge.at<double_t>(i, i - __point_width + 1) = 1e7;
                __edge.at<double_t>(i - __point_width + 1, i) = 1e7;
            }
            
            if (i + 1 < __point_height * __point_width)
            {
                __edge.at<double_t>(i, i + 1) = 1e7;
                __edge.at<double_t>(i + 1, i) = 1e7;
            }
            
            if (i + __point_width + 1 < __point_height * __point_width)
            {
                __edge.at<double_t>(i, i + __point_width + 1) = 1e7;
                __edge.at<double_t>(i + __point_width + 1, i) = 1e7;
            }
        }
    }
}


void Graph::updateEdge(vector<Point2i> &obstacles)
{
    vector<bool> isObstacles;
    isObstacles.reserve(__vertexNum);
    for (int i = 0; i < __vertexNum; i++)
    {
        isObstacles.push_back(0);
    }
  
    for (std::vector<cv::Point2i>::size_type i = 0; i < obstacles.size(); i++)
    {
        int obstacleNum = obstacles[i].y * __point_width + obstacles[i].x;
        isObstacles[obstacleNum] = 1;
        for (int j = 0; j < __vertexNum; j++)
        {
            __edge.at<double_t>(obstacleNum, j) = 1e7;
            __edge.at<double_t>(j, obstacleNum) = 1e7;
        }
    }
 
    for (int i = 0; i < __vertexNum; i++)
    {
        
        if ((i % __point_width != 0) && (i % __point_width != __point_width - 1) &&
            (i / __point_width != 0) && (i / __point_width != __point_height - 1))
        {
            
            if (isObstacles[i + 1] == 1 && isObstacles[i + __point_width] == 1)
            {
                __edge.at<double_t>(i, i + __point_width + 1) = 1e7;
                __edge.at<double_t>(i + __point_width + 1, i) = 1e7;
            }
           
            if (isObstacles[i + 1] == 1 && isObstacles[i - __point_width] == 1)
            {
                __edge.at<double_t>(i, i - __point_width + 1) = 1e7;
                __edge.at<double_t>(i - __point_width + 1, i) = 1e7;
            }
           
            if (isObstacles[i - 1] == 1 && isObstacles[i + __point_width] == 1)
            {
                __edge.at<double_t>(i, i + __point_width - 1) = 1e7;
                __edge.at<double_t>(i + __point_width - 1, i) = 1e7;
            }
           
            if (isObstacles[i - 1] == 1 && isObstacles[i - __point_width] == 1)
            {
                __edge.at<double_t>(i, i - __point_width - 1) = 1e7;
                __edge.at<double_t>(i - __point_width - 1, i) = 1e7;
            }
        }
       
        if (i == __point_width)
        {
            if (isObstacles[i + 1] == 1 && isObstacles[i - __point_width] == 1)
            {
                __edge.at<double_t>(i, i - __point_width + 1) = 1e7;
                __edge.at<double_t>(i - __point_width + 1, i) = 1e7;
            }
        }
     
        if (i == __point_width + __point_width - 1)
        {
            if (isObstacles[i - 1] == 1 && isObstacles[i - __point_width] == 1)
            {
                __edge.at<double_t>(i, i - __point_width - 1) = 1e7;
                __edge.at<double_t>(i - __point_width - 1, i) = 1e7;
            }
        }
        if (i == __point_width * __point_height - __point_width + 1)
        {
            if (isObstacles[i - 1] == 1 && isObstacles[i - __point_width] == 1)
            {
                __edge.at<double_t>(i, i - __point_width - 1) = 1e7;
                __edge.at<double_t>(i - __point_width - 1, i) = 1e7;
            }
        }
        if (i == __point_width * __point_height - 2)
        {
            if (isObstacles[i + 1] == 1 && isObstacles[i - __point_width] == 1)
            {
                __edge.at<double_t>(i, i - __point_width + 1) = 1e7;
                __edge.at<double_t>(i - __point_width + 1, i) = 1e7;
            }
        }
    }
}
